package pack.model.controllers;

import pack.help.State;
import pack.help.Vec;
import pack.model.RealLifeProperties;

public class PTController implements Controller {

	double C = 0.25;
	
	public double[] control(State start, State goal, Vec dir, float ts) {
		double ret[] = new double[2];
		
		float goalV = (goal.v1 + goal.v2)/2f;
		float startV = (start.v1 + start.v2)/2f;
		float goalOmega = (goal.v2-goal.v1)/(2f*RealLifeProperties.l);
		
		// Error calculation
		double eX = (goal.pos.x-start.pos.x)*Math.cos(goal.fi) + 
					(goal.pos.y-start.pos.y)*Math.sin(goal.fi);
		double eY = (goal.pos.x-start.pos.x)*Math.sin(goal.fi) +
					(goal.pos.y-start.pos.y)*Math.cos(goal.fi);
		double eF = goal.fi-start.fi;
		
		// Velocity control
		double eXder = goalV-startV*Math.cos(eF)+goalOmega*eY;
		double vS = eXder + Math.sqrt(2*RealLifeProperties.maxA*Math.abs(eX))*Math.signum(eX);
		double outV = vS/ts;
		
		// Steering control
		double fiP = goal.fi + Math.atan(3*C*Math.pow(eY/C, 2/3)*Math.signum(eY));
		double eYder  = -goalOmega*eX+startV*Math.sin(eF);
		double omegaP = goalOmega + (2*Math.pow(eY/C, -1/3))*eYder*Math.signum(eY)/(1+Math.pow(Math.tan(fiP-goal.fi), 2));
		
		double omegaS = omegaP+Math.sqrt(2*RealLifeProperties.maxAlpha*Math.abs(fiP-start.fi))*Math.signum(fiP-start.fi);
		
		ret[0] = outV - RealLifeProperties.l*omegaS/ts-start.v1;
		ret[1] = outV + RealLifeProperties.l*omegaS/ts-start.v2;
		return ret;
	}

}
